#include "main.h"
#include "motor_encoder.h"
#include "cal_rpm.h"
#include "motor_gpio.h"
#include "motor_pwm.h"
#include "timer.h"
#include "rt_wrapper.h"
#include "mq_wrapper.h"

#include "wasm_export.h"
#include "bh_read_file.h"

// 记录运行次数
int current_time = 0;
int max_run_time = 100; // 运行次数:100 * 20ms = 2s
pthread_mutex_t running_mutex = PTHREAD_MUTEX_INITIALIZER;
char output_file_name[50] = "default_data_output.txt";
const char *horizontal_line = "------------------------------------------";
int running = 0;

uint8 *wasm_file_buffer;
uint32 file_buf_size;
char error_buffer[128];

wasm_module_t module;
wasm_module_inst_t module_inst;
wasm_function_inst_t func;
wasm_exec_env_t exec_env;

// 控制线程的执行函数：设置为实时线程后，进入可唤醒休眠。
// 控制线程是四个线程里，唯一可以接收SIGALRM信号的线程。
void *control_thread_func(wasm_exec_env_t exec_env, void *arg)
{
    static char data_buffer[100];
    wasm_module_inst_t module_inst_thread = get_module_inst(exec_env);
    wasm_function_inst_t func_thread;
    set_sched(SCHED_FIFO, 99);
    wasm_runtime_init_thread_env();
    while (1)
    {
        pause();
        if (running)
        {
            // 获取电机转速
            double speed_rpm = calculate_rpm(get_and_clear_encoder_edge_count());
            // 调用pid_controller.wasm里的pid_controller函数
            func_thread = wasm_runtime_lookup_function(module_inst_thread, "pid_controller", NULL);
            wasm_val_t results[1], args[1];
            args[0].kind = WASM_F64;
            args[0].of.f64 = speed_rpm;
            wasm_runtime_call_wasm_a(exec_env, func_thread, 1, results, 1, args);
            //  给PID控制器传入转速参数，得到输出占空比
            double duty_cycle = results[0].of.f64;
            // 设置输出PWM的占空比
            set_pwm_output(duty_cycle);
            // 将时间、输出占空比、电机转速写入缓冲区，然后通过消息队列发送给数据记录进程
            sprintf(data_buffer, "%f %f %f\n", (current_time + 1) * (SAMPLE_PERIOD / 1000.0 / 1000.0), duty_cycle, speed_rpm);
            int ret = mq_wrapper_send_data(data_buffer);
            if (ret == -1)
                printf("发送消息“%s”失败\n", data_buffer);
            // 若达到了设定的运行次数，则向数据记录进程发送"close"命令，让数据记录进程关闭输出文件。并将PWM输出设置为0（停止电机）后退出程序。
            if (++current_time > max_run_time - 1)
            {
                pthread_mutex_lock(&running_mutex);
                running = 0;
                pthread_mutex_unlock(&running_mutex);
            }
        }
        else
        {
            while (mq_wrapper_send_close_file_cmd() == -1)
                ;
            set_pwm_output(0);
            stop_timer();
        }
    }
    wasm_runtime_destroy_thread_env();
}

void print_params()
{
    puts(horizontal_line);
    wasm_val_t results[1];
    func = wasm_runtime_lookup_function(module_inst, "get_open_loop", NULL);
    wasm_runtime_call_wasm_a(exec_env, func, 1, results, 0, NULL);

    if (results[0].of.i32)
    {
        func = wasm_runtime_lookup_function(module_inst, "get_open_loop_duty_cycle", NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 1, results, 0, NULL);

        printf("开环控制, 开环PWM占空比:%f%%\n", results[0].of.f64);
    }
    else
        puts("闭环控制");
    printf("电机转向:%d(0正1反)\n", direction);
    printf("最大运行时间:%dms\n", max_run_time * SAMPLE_PERIOD / 1000);
    printf("输出文件:%s\n", output_file_name);
    double Kp, Ki, Kd;
    func = wasm_runtime_lookup_function(module_inst, "get_Kp", NULL);
    wasm_runtime_call_wasm_a(exec_env, func, 1, results, 0, NULL);
    Kp = results[0].of.f64;
    func = wasm_runtime_lookup_function(module_inst, "get_Ki", NULL);
    wasm_runtime_call_wasm_a(exec_env, func, 1, results, 0, NULL);
    Ki = results[0].of.f64;
    func = wasm_runtime_lookup_function(module_inst, "get_Kd", NULL);
    wasm_runtime_call_wasm_a(exec_env, func, 1, results, 0, NULL);
    Kd = results[0].of.f64;
    printf("Kp: %f, Ki: %f, Kd: %f\n", Kp, Ki, Kd);
    func = wasm_runtime_lookup_function(module_inst, "get_target_speed", NULL);
    wasm_runtime_call_wasm_a(exec_env, func, 1, results, 0, NULL);
    printf("目标速度: %f\n", results[0].of.f64);
    puts(horizontal_line);
}

int parse_and_exec_user_input_cmd(const char *cmd)
{
    if (strcmp(cmd, "stop") == 0)
    {
        pthread_mutex_lock(&running_mutex);
        running = 0;
        pthread_mutex_unlock(&running_mutex);
        return 0;
    }
    else if (strcmp(cmd, "param") == 0)
    {
        print_params();
        return 0;
    }
    if (running)
    {
        printf("电机运行中，无法执行该命令\n");
        return -1;
    }
    if (strncmp(cmd, "ol:", 3) == 0)
    {
        func = wasm_runtime_lookup_function(module_inst, "set_open_loop", NULL);
        wasm_val_t args[1];
        args[0].kind = WASM_I32;
        args[0].of.i32 = atoi(cmd + 3);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);
        return 0;
    }
    else if (strncmp(cmd, "old:", 4) == 0)
    {
        func = wasm_runtime_lookup_function(module_inst, "set_open_loop_duty_cycle", NULL);
        wasm_val_t args[1];
        args[0].kind = WASM_F64;
        args[0].of.f64 = strtod(cmd + 4, NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);
        return 0;
    }
    else if (strncmp(cmd, "dir:", 4) == 0)
    {
        set_motor_direction(atoi(cmd + 4));
        return 0;
    }
    else if (strncmp(cmd, "time:", 5) == 0)
    {
        max_run_time = strtod(cmd + 5, NULL) / SAMPLE_PERIOD * 1000.0;
        return 0;
    }
    else if (strncmp(cmd, "file:", 5) == 0)
    {
        strcpy(output_file_name, cmd + 5);
        return 0;
    }
    else if (strncmp(cmd, "p:", 2) == 0)
    {
        func = wasm_runtime_lookup_function(module_inst, "set_Kp", NULL);
        wasm_val_t args[1];
        args[0].kind = WASM_F64;
        args[0].of.f64 = strtod(cmd + 2, NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);
        return 0;
    }
    else if (strncmp(cmd, "i:", 2) == 0)
    {
        func = wasm_runtime_lookup_function(module_inst, "set_Ki", NULL);
        wasm_val_t args[1];
        args[0].kind = WASM_F64;
        args[0].of.f64 = strtod(cmd + 2, NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);
        return 0;
    }
    else if (strncmp(cmd, "d:", 2) == 0)
    {
        func = wasm_runtime_lookup_function(module_inst, "set_Kd", NULL);
        wasm_val_t args[1];
        args[0].kind = WASM_F64;
        args[0].of.f64 = strtod(cmd + 2, NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);
        return 0;
    }
    else if (strncmp(cmd, "target:", 7) == 0)
    {
        func = wasm_runtime_lookup_function(module_inst, "set_target_speed", NULL);
        wasm_val_t args[1];
        args[0].kind = WASM_F64;
        args[0].of.f64 = strtod(cmd + 7, NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);
        return 0;
    }
    else if (strncmp(cmd, "start", 5) == 0)
    {
        current_time = 0;
        running = 1;
        get_and_clear_encoder_edge_count();
        func = wasm_runtime_lookup_function(module_inst, "reset_controller_state", NULL);
        wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 0, NULL);
        print_params();
        mq_wrapper_send_open_file_cmd(output_file_name);
        start_timer(SAMPLE_PERIOD);
        return 0;
    }
    else
    {
        puts("未知指令");
        return -1;
    }
}

int main(int argc, char *argv[])
{
    if (argc != 2)
    {
        printf("请输入PID控制器的wasm模块\n");
        return -1;
    }
    wasm_thread_t control_thread;

    // 初始化消息队列
    mq_wrapper_init();
    // 锁定内存
    lock_memory();
    // 初始化GPIO并设置电机转向
    init_gpio();
    set_motor_direction(0);
    // 初始化PWM
    init_pwm();
    // 加载wasm模块
    static char wasm_global_heap_buf[512 * 1024];
    RuntimeInitArgs init_args;
    memset(&init_args, 0, sizeof(RuntimeInitArgs));
    init_args.mem_alloc_type = Alloc_With_Pool;
    init_args.mem_alloc_option.pool.heap_buf = wasm_global_heap_buf;
    init_args.mem_alloc_option.pool.heap_size = sizeof(wasm_global_heap_buf);
    init_args.max_thread_num = 4;

    if (!wasm_runtime_full_init(&init_args))
    {
        return -1;
    }

    wasm_file_buffer = (uint8 *)bh_read_file_to_buffer(argv[1], &file_buf_size);
    printf("bh_read_file_to_buffer. file_buf_size: %u\n", file_buf_size);

    module = wasm_runtime_load(wasm_file_buffer, file_buf_size, error_buffer, sizeof(error_buffer));
    printf("wasm_runtime_load\n");

    module_inst = wasm_runtime_instantiate(module, 8092, 8092, error_buffer, sizeof(error_buffer));
    printf("wasm_runtime_instantiate\n");

    /* 创建模块的运行环境 */
    exec_env = wasm_runtime_create_exec_env(module_inst, 8092);
    printf("wasm_runtime_create_exec_env\n");
    // 设置PID控制器采样周期
    func = wasm_runtime_lookup_function(module_inst, "set_sample_period_us", NULL);
    wasm_val_t args[1];
    args[0].kind = WASM_I32;
    args[0].of.i32 = SAMPLE_PERIOD;
    wasm_runtime_call_wasm_a(exec_env, func, 0, NULL, 1, args);

    // 创建控制线程
    wasm_runtime_spawn_thread(exec_env, &control_thread, control_thread_func, NULL);
    //pthread_create(&control_thread, NULL, control_thread_func, exec_env);

    // 创建编码器输入边缘的读取线程
    init_encoder_poll_threads();
    // 主线程应当屏蔽定时器的SIGALRM信号，让控制线程运行信号处理函数
    mask_sig_alarm();
    printf("初始化完毕\n");

    char cmd_buf[50];
    while (1)
    {
        // 获取一行用户的指令
        fgets(cmd_buf, sizeof(cmd_buf), stdin);
        // 去掉换行符
        size_t length = strlen(cmd_buf);
        if (length > 0 && cmd_buf[length - 1] == '\n')
            cmd_buf[length - 1] = '\0';
        parse_and_exec_user_input_cmd(cmd_buf);
    }

    wasm_runtime_destroy_exec_env(exec_env);
    wasm_runtime_deinstantiate(module_inst);
    wasm_runtime_unload(module);
    wasm_runtime_destroy();
    return 0;
}
